/** handlers.c: functions for installing and uninstalling handlers
 *
 * Author: 	Nora Tarano <ntarano@andrew.cmu.edu>
 *          Yuan Meng <yuanm@andrew.cmu.edu>
 *          Yipeng Yun <yipengy@andrew.cmu.edu>
 * Date: 	Dec 11, 2011  
 */

#include <types.h>
#include <exports.h>

#define LDR_PC_BASE		0xe51ff000 // ldr pc, [pc, #imm12]
#define LDR_U_FLAG		0x00800000
#define LDR_IMM_MASK	0x00000fff
#define LDR_PC_NEXT		0xe51ff004 // ldr pc, [pc, #-4]
#define PC_OFFSET		0x8

// Hijacks U-boot's handler to replace with new one
int install_handler(unsigned int routine, unsigned int *vector, unsigned int *instruction)
{
    unsigned int vector_instr = *vector;	
	unsigned int *old_handler;
	long offset;

	// Verify that vectors contains "ldr pc, [pc, #imm12]"
	if ((vector_instr & ~(LDR_U_FLAG | LDR_IMM_MASK)) != LDR_PC_BASE) {
		printf("Kernel panic: unrecognized vector: %u\n", vector_instr);
		return -1;
	}

	// Determine the literal offset
	if (vector_instr & LDR_U_FLAG)
		offset = vector_instr & LDR_IMM_MASK;
	else
		offset = -(vector_instr & LDR_IMM_MASK);
		
	// Find the location of U-Boot's handler
	old_handler = *(unsigned int **)((unsigned int)vector + PC_OFFSET + offset);
	
	// Save the first two instructions for later restoration
	instruction[0] = *(old_handler);
	instruction[1] = *(old_handler+1);

	// Redirect U-Boot's SWI and IRQ handlers to ours
	*(old_handler) = LDR_PC_NEXT;
	*(old_handler+1) = routine;
	
	return 0;
}

// Replaces custom handler with U-boot's handler
int uninstall_handler(unsigned int *vector, unsigned int *instruction)
{
    unsigned int vector_instr = *vector;
    unsigned int *handler;
    long offset;
    
    // Verify that vectors contains "ldr pc, [pc, #imm12]"
	if ((vector_instr & ~(LDR_U_FLAG | LDR_IMM_MASK)) != LDR_PC_BASE) {
		printf("Kernel panic: unrecognized vector: %u\n", vector_instr);
		return -1;
	}
    
    // Determine the literal offset
	if (vector_instr & LDR_U_FLAG)
		offset = vector_instr & LDR_IMM_MASK;
	else
		offset = -(vector_instr & LDR_IMM_MASK);
    
    // Find the location of the handler to replace
	handler = *(unsigned int **)((unsigned int)vector + PC_OFFSET + offset);
    
    *(handler) = instruction[0];
    *(handler+1) = instruction[1];
    
    return 0;
}
